#include <cerrno>
#include <csignal>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <signal.h>
#include "SerialPort.h"
#include "SignalDispatcher.h"

namespace uav
{
namespace comm
{

void SerialPort::open(BaudRate baudRate, CharSize charSize, Parity parity,
		StopBits stopBits, FlowControl flowControl)
		throw(SerialPort::OpenFailed, SerialPort::UnsupportedBaudRate, std::invalid_argument)
{
	assert(!isOpen());

	fileDescriptor = open(portName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
	if (fileDescriptor < 0)
		throw OpenFailed(std::strerror(errno));

	SignalDispatcher &dispatcher = SignalDispatcher::instance();
	dispatcher.attachHandler(SIGIO, *this);

	// Direct SIGURG and SIGIO signals for the port to the current process
	if (fcntl(fileDescriptor, F_SETOWN, getpid()) < 0)
		throw OpenFailed(std::strerror(errno));

	// Enable asynchronous I/O
	if (fcntl(fileDescriptor, F_SETFL, FASYNC) < 0)
		throw OpenFailed(std::strerror(errno));

	// Save the current settings of the port so they can be restored on close
	if (tcgetattr(fileDescriptor, &oldPortSettings) < 0)
		throw OpenFailed(std::strerror(errno));

	// Set up new port settings

	termios portSettings;
	bzero(&portSettings, sizeof(portSettings));

	// Enable receiver and ignore modem control lines
	portSettings.c_cflag |= CREAD | CLOCAL;

	// Make read return immediately with only the currently available characters
	portSettings.c_cc[VMIN]  = 0;
	portSettings.c_Cc[VTIME] = 0;

	// Flush the port's input buffer
	if (tcflush(fileDescriptor, TCIFLUSH) < 0)
		throw OpenFailed(std::strerror(errno));

	// Write the new settings to the port
	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw OpenFailed(std::strerror(errno));

	opened = true;

	// Now that the port is open, set the user-defined options
	setBaudRate(baudRate);
	setCharSize(charSize);
	setParity(parity);
	setStopBits(stopBits);
	setFlowControl(flowControl);
}

void SerialPort::close()
{
	assert(isOpen());

	SignalDispatcher &dispatcher = SignalDispatcher::instance();
	dispatcher.detachHandler(SIGIO, *this);

	// Restore old port settings
	tcsetattr(fileDescriptor, TCSANOW, &oldPortSettings);

	close(fileDescriptor);
	opened = false;
}

void SerialPort::setBaudRate(SerialPort::BaudRate baudRate)
		throw(SerialPort::UnsupportedBaudRate, std::invalid_argument, std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	// Set both the input and output baud rates
	if ((cfsetispeed(&portSettings, baudRate) < 0)
			|| (cfsetospeed(&portSettings, baudRate) < 0))
		throw UnsupportedBaudRate();

	// Write the new baud rates to the port
	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw UnsupportedBaudRate(strerror(errno));
}

SerialPort::BaudRate SerialPort::getBaudRate() const throw(std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	return SerialPort::BaudRate(cfgetispeed(&portSettings));
}

void SerialPort::setCharSize(SerialPort::CharSize charSize)
		throw(std::invalid_argument, std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	portSettings.c_cflag &= ~CSIZE;
	portSettings.c_cflag |= charSize;

	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw std::invalid_argument(strerror(errno));
}

SerialPort::CharSize SerialPort::getCharSize() const throw(std::runtime_error)
{
	if (!isOpen())
		throw SerialPort::NotOpen();

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	return SerialPort::BaudRate(portSettings.c_cflag & CSIZE);
}

void SerialPort::setParity(SerialPort::Parity parity)
		throw(std::invalid_argument, std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	switch (parity) {
		case PARITY_EVEN:
			portSettings.c_cflag |= PARENB;
			portSettings.c_cflag &= ~PARODD;
			portSettings.c_iflag |= INPCK;
			break;
		case PARITY_ODD:
			portSettings.c_cflag |= (PARENB | PARODD);
			portSettings.c_iflag |= INPCK;
			break;
		case PARITY_NONE:
			portSettings.c_cflag &= ~PARENB;
			portSettings.c_iflag |= IGNPAR;
			break;
		default:
			throw std::invalid_argument("Invalid parity setting.");
	}

	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw std::invalid_argument(strerror(errno));
}

SerialPort::Parity SerialPort::getParity() const throw(std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	if (portSettings.c_cflag & PARENB) {
		if (portSettings.c_cflag & PARODD)
			return PARITY_ODD;
		else
			return PARITY_EVEN;
	}
	return PARITY_NONE;
}

void SerialPort::setStopBits(SerialPort::StopBits stopBits)
		throw(std::invalid_argument, std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	switch (stopBits) {
		case STOP_BITS_1:
			portSettings.c_cflag &= ~CSTOPB;
			break;
		case STOP_BITS_2:
			portSettings.c_cflag |= CSTOPB;
			break;
		default:
			throw std::invalid_argument("Invalid number of stop bits.");
	}

	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw std::invalid_argument(strerror(errno));
}

SerialPort::StopBits SerialPort::getStopBits() const throw(std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	if (portSettings.c_cflag & CSTOPB)
		return STOP_BITS_2;
	return STOP_BITS_1;
}

void SerialPort::setFlowControl(SerialPort::FlowControl flowControl)
		throw(std::invalid_argument, std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	switch (flowControl) {
		case FLOW_CONTROL_HARD:
			portSettings.c_cflag |= CRTSCTS;
			break;
		case FLOW_CONTROL_NONE:
			portSettings.c_cflag &= ~CRTSCTS;
			break;
		default:
			throw std::invalid_argument("Invalid flow control setting.");
	}

	if (tcsetattr(fileDescriptor, TCSANOW, &portSettings) < 0)
		throw std::invalid_argument(strerror(errno));
}

SerialPort::FlowControl SerialPort::getFlowControl() const throw(std::runtime_error)
{
	assert(isOpen());

	termios portSettings;
	if (tcgetattr(fileDescriptor, &portSettings) < 0)
		throw std::runtime_error(strerror(errno));

	if (portSettings.c_cflag & CRTSCTS)
		return FLOW_CONTROL_HARD;
	return FLOW_CONTROL_NONE;
}

bool SerialPort::isDataAvailable() const
{
	assert(isOpen());

	return (!inputBuffer.empty());
}

namespace
{
	// Number of microseconds to sleep while waiting for data.
	const __useconds_t SERIAL_SLEEP_TIME = 1000;
}

uint8_t SerialPort::readByte()
{
	assert(isOpen());

	while (inputBuffer.empty()) usleep(SERIAL_SLEEP_TIME);

	uint8_t nextChar = inputBuffer.front();
	inputBuffer.pop();
	return nextChar;
}

void SerialPort::read(std::vector<uint8_t> &buffer, int numBytes)
{
	assert(isOpen());

	buffer.resize(0);
	if (numBytes == 0) {
		while (!inputBuffer.empty()) {
			buffer.push_back(inputBuffer.front());
			inputBuffer.pop();
		}
	} else {
		buffer.reserve(numBytes);
		for (int i = 0; i < numBytes; i++) {
			while (inputBuffer.empty()) usleep(SERIAL_SLEEP_TIME);
			buffer.push_back(inputBuffer.front());
			inputBuffer.pop();
		}
	}
}

std::string SerialPort::readLine(char terminator)
{
	assert(isOpen());

	std::string res;

	char nextChar = 0;
	do {
		while (inputBuffer.empty()) usleep(SERIAL_SLEEP_TIME);
		nextChar = inputBuffer.front();
		inputBuffer.pop();
		res += nextChar;
	} while (nextChar != terminator);

	return res;
}

bool SerialPort::write(const uint8_t *buffer, int size)
{
	while (size > 0) {
		int written;

		do {
			written = write(fileDescriptor, buffer, size);
		} while ((written < 0) && (errno == EAGAIN));

		if (written < 0)
			return false;

		size -= written;
		buffer += written;
	}
	return true;
}

bool SerialPort::writeByte(uint8_t dataByte)
{
	assert(isOpen());

	return write(&dataByte, 1);
}

bool SerialPort::write(const std::vector<uint8_t> &buffer)
{
	assert(isOpen());

	if (buffer.empty()) return;

	return write(&buffer[0], buffer.size());
}

bool SerialPort::write(const std::string &str)
{
	assert(isOpen());

	return write(reinterpret_cast<const uint8_t *>(str.c_str()), str.length());
}

void SerialPort::handleSignal(int sigNum)
{
	if (sigNum != SIGIO) return;

	int numBytesAvailable;
	if (ioctl(fileDescriptor, FIONREAD, &numBytesAvailable) < 0) return;

	for (int i = 0; i < numBytesAvailable; i++) {
		uint8_t nextByte;
		if (read(fileDescriptor, &nextByte, 1) > 0)
			inputBuffer.push(nextByte);
		else
			break;
	}
}

} // end namespace comm
} // end namespace uav
